#include "troycontroller.h"

#include <QDebug>

#define NUM_OF_JOINTS   4
#define RIGHT_JOINTS    {16,17,24,25}
#define LEFT_JOINTS     {0,1,8,9}

TroyController::TroyController(Troy* model, QObject *parent) :
    QObject(parent),
    _model(model)
{}

void TroyController::mirror(MirroringOption option, int jointID){
    int i, mirroredJoint;
    int leftJoints[NUM_OF_JOINTS] = LEFT_JOINTS;
    int rightJoints[NUM_OF_JOINTS] = RIGHT_JOINTS;
    int *originalJoints, *mirroredJoints;
    float theta;
    bool changed = false;
    SetRobotJointCommand* jointCommand;
    RobotCommandGroup* mirrorCommand;

    if(option == JOINT){
        //Just mirror the joint and return
        theta = _model->jointAngle(jointID);
        mirroredJoint = jointID > 16 ? jointID - 16 : jointID + 16;

        //Check to see if mirroring actually changes the angle.
        //If it does, then create a command, execute it, and stick
        //it on the history
        if(theta != _model->jointAngle(mirroredJoint)){
            jointCommand = new SetRobotJointCommand(_model, mirroredJoint, theta);
            jointCommand->execute();
            History::instance()->push(jointCommand);
        }

        return;
    }
    else if(option == LEFT_TO_RIGHT){
        originalJoints = leftJoints;
        mirroredJoints = rightJoints;
    }
    else if(option == RIGHT_TO_LEFT){
        originalJoints = rightJoints;
        mirroredJoints = leftJoints;
    }

    //We are mirroring an entire side
    mirrorCommand = new RobotCommandGroup(_model);
    for(i = 0; i < NUM_OF_JOINTS; i++){
        theta = _model->jointAngle(originalJoints[i]);
        mirroredJoint = mirroredJoints[i];

        //Only create a joint command if it will make a difference
        if(theta != _model->jointAngle(mirroredJoint)){
            jointCommand = new SetRobotJointCommand(_model, mirroredJoint, theta);
            mirrorCommand->append(jointCommand);
            changed = true;
        }
    }

    //Only execute and store the mirror command if it will actually
    //change something
    if(changed){
        mirrorCommand->execute();
        History::instance()->push(mirrorCommand);
    }
    else{
        delete mirrorCommand;
    }
}

void TroyController::mirror(RobotComponent *component){
//    int i;
//    QList<int> jointIDs = component->getJointIDs();

//    for(i = 0; i < jointIDs.length(); i++){
//        mirror(JOINT, jointIDs.at(i));
//    }
}
